#include "motor.h"
#include "stm32f10x_gpio.h"
#include "stm32f10x_rcc.h"
#include <stdio.h>

extern void USART_SendString(USART_TypeDef* USARTx, uint8_t *str);

// 电机GPIO配置
void Motor_GPIO_Configuration(void)
{
    GPIO_InitTypeDef GPIO_InitStructure;
    
    // 使能GPIOB时钟
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
    
    // 配置所有电机控制引脚为推挽输出
    GPIO_InitStructure.GPIO_Pin = ENA_PIN | ENB_PIN | IN1_PIN | IN2_PIN | IN3_PIN | IN4_PIN;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_Init(GPIOB, &GPIO_InitStructure);
    
    // 确保所有引脚初始为低电平
    GPIO_ResetBits(GPIOB, ENA_PIN);
    GPIO_ResetBits(GPIOB, ENB_PIN);
    GPIO_ResetBits(GPIOB, IN1_PIN);
    GPIO_ResetBits(GPIOB, IN2_PIN);
    GPIO_ResetBits(GPIOB, IN3_PIN);
    GPIO_ResetBits(GPIOB, IN4_PIN);
    
    // 添加初始化完成信息 - 诊断步骤1
    USART_SendString(USART1, (uint8_t*)"Motor GPIO Init Complete - All pins LOW\r\n");
    Motor_Stop();
}

// 读取并显示电机引脚状态
void Motor_Debug_Status(void)
{
    uint8_t status_msg[64];
    uint8_t ena = GPIO_ReadOutputDataBit(GPIOB, ENA_PIN);
    uint8_t enb = GPIO_ReadOutputDataBit(GPIOB, ENB_PIN);
    uint8_t in1 = GPIO_ReadOutputDataBit(GPIOB, IN1_PIN);
    uint8_t in2 = GPIO_ReadOutputDataBit(GPIOB, IN2_PIN);
    uint8_t in3 = GPIO_ReadOutputDataBit(GPIOB, IN3_PIN);
    uint8_t in4 = GPIO_ReadOutputDataBit(GPIOB, IN4_PIN);
    
    sprintf((char*)status_msg, "Motor Status: ENA=%d,ENB=%d,IN1=%d,IN2=%d,IN3=%d,IN4=%d\r\n",
            ena, enb, in1, in2, in3, in4);
    USART_SendString(USART1, status_msg);
}

// 电机前进 - 两个轮子同时前进
void Motor_Forward(void)
{
    // 添加调试信息
    USART_SendString(USART1, (uint8_t*)"DEBUG: Motor Forward\r\n");
    
    // 1. 先停止所有电机
    Motor_Stop();
    
    // 2. 设置右轮方向：前转 (IN1=0, IN2=1)
    GPIO_ResetBits(GPIOB, IN1_PIN);  // IN1 = 0 (GND)
    GPIO_SetBits(GPIOB, IN2_PIN);    // IN2 = 1 (3.3V)
    
    // 3. 设置左轮方向：前转 (IN3=0, IN4=1)
    GPIO_ResetBits(GPIOB, IN3_PIN);  // IN3 = 0 (GND)
    GPIO_SetBits(GPIOB, IN4_PIN);    // IN4 = 1 (3.3V)
    
    // 4. 短暂延时确保方向信号稳定
    Delay(5000);
    
    // 5. 同时使能两个电机 - 确保两个轮子都转动
    GPIO_SetBits(GPIOB, ENA_PIN);  // 右轮使能
    GPIO_SetBits(GPIOB, ENB_PIN);  // 左轮使能
    
    // 确认使能状态
    USART_SendString(USART1, (uint8_t*)"DEBUG: ENA=HIGH, ENB=HIGH (Both wheels enabled)\r\n");
    
    // USART_SendString(USART1, (uint8_t*)"FORWARD: Both wheels should spin\r\n");
    Motor_Debug_Status();
}

// 电机后退 - 两个轮子同时后退
void Motor_Backward(void)
{
    // 添加调试信息
    USART_SendString(USART1, (uint8_t*)"DEBUG: Motor Backward\r\n");
    
    // 1. 先停止所有电机
    Motor_Stop();
    
    // 2. 设置右轮方向：后转 (IN1=1, IN2=0)
    GPIO_SetBits(GPIOB, IN1_PIN);    // IN1 = 1 (3.3V)
    GPIO_ResetBits(GPIOB, IN2_PIN);  // IN2 = 0 (GND)
    
    // 3. 设置左轮方向：后转 (IN3=1, IN4=0)
    GPIO_SetBits(GPIOB, IN3_PIN);    // IN3 = 1 (3.3V)
    GPIO_ResetBits(GPIOB, IN4_PIN);  // IN4 = 0 (GND)
    
    // 4. 短暂延时确保方向信号稳定
    Delay(5000);
    
    // 5. 同时使能两个电机 - 确保两个轮子都转动
    GPIO_SetBits(GPIOB, ENA_PIN);  // 右轮使能
    GPIO_SetBits(GPIOB, ENB_PIN);  // 左轮使能
    
    // 确认使能状态
    USART_SendString(USART1, (uint8_t*)"DEBUG: ENA=HIGH, ENB=HIGH (Both wheels enabled)\r\n");
    
    // USART_SendString(USART1, (uint8_t*)"BACKWARD: Both wheels should spin\r\n");
}

// 电机左转 - 右轮前进，左轮停止
void Motor_Left(void)
{
    // 添加调试信息
    USART_SendString(USART1, (uint8_t*)"DEBUG: Motor Left\r\n");
    
    // 1. 先停止所有电机
    Motor_Stop();
    
    // 2. 设置右轮方向：前转 (IN1=0, IN2=1)
    GPIO_ResetBits(GPIOB, IN1_PIN);  // IN1 = 0
    GPIO_SetBits(GPIOB, IN2_PIN);    // IN2 = 1
    
    // 3. 左轮方向引脚全部置0 (停止状态)
    GPIO_ResetBits(GPIOB, IN3_PIN | IN4_PIN);
    
    // 4. 短暂延时确保方向信号稳定
    Delay(5000);
    
    // 5. 只使能右轮电机
    GPIO_SetBits(GPIOB, ENA_PIN);    // 右轮转动
    GPIO_ResetBits(GPIOB, ENB_PIN);  // 左轮停止
    
    // 确认使能状态
    USART_SendString(USART1, (uint8_t*)"DEBUG: ENA=HIGH, ENB=LOW (Right wheel only)\r\n");
    
    // USART_SendString(USART1, (uint8_t*)"LEFT: Right wheel should spin\r\n");
}

// 电机右转 - 左轮前进，右轮停止
void Motor_Right(void)
{
    // 添加调试信息
    USART_SendString(USART1, (uint8_t*)"DEBUG: Motor Right\r\n");
    
    // 1. 先停止所有电机
    Motor_Stop();
    
    // 2. 设置左轮方向：前转 (IN3=0, IN4=1)
    GPIO_ResetBits(GPIOB, IN3_PIN);  // IN3 = 0
    GPIO_SetBits(GPIOB, IN4_PIN);    // IN4 = 1
    
    // 3. 右轮方向引脚全部置0 (停止状态)
    GPIO_ResetBits(GPIOB, IN1_PIN | IN2_PIN);
    
    // 4. 短暂延时确保方向信号稳定
    Delay(5000);
    
    // 5. 只使能左轮电机
    GPIO_SetBits(GPIOB, ENB_PIN);    // 左轮转动
    GPIO_ResetBits(GPIOB, ENA_PIN);  // 右轮停止
    
    // 确认使能状态
    USART_SendString(USART1, (uint8_t*)"DEBUG: ENA=LOW, ENB=HIGH (Left wheel only)\r\n");
    
    // USART_SendString(USART1, (uint8_t*)"RIGHT: Left wheel should spin\r\n");
}

// 电机停止 - 所有电机停止
void Motor_Stop(void)
{
    // 添加调试信息
    USART_SendString(USART1, (uint8_t*)"DEBUG: Motor Stop\r\n");
    
    // 禁用使能引脚 - 确保两个轮子都停止
    GPIO_ResetBits(GPIOB, ENA_PIN);  // 右轮禁用
    GPIO_ResetBits(GPIOB, ENB_PIN);  // 左轮禁用
    
    // 确认禁用状态
    USART_SendString(USART1, (uint8_t*)"DEBUG: ENA=LOW, ENB=LOW (Both wheels disabled)\r\n");
    
    // 短暂延时
    Delay(500);
    
    // 复位所有方向引脚
    GPIO_ResetBits(GPIOB, IN1_PIN);
    GPIO_ResetBits(GPIOB, IN2_PIN);
    GPIO_ResetBits(GPIOB, IN3_PIN);
    GPIO_ResetBits(GPIOB, IN4_PIN);
}

// 简单延时函数
void Delay(__IO uint32_t nCount)
{
    for(; nCount != 0; nCount--);
}